#include <rtthread.h>
#include <rthw.h>
#include <stdio.h>
#include <stdlib.h>
#include "../DeviceDrivers/stm32_drv_can.h"
#include "./ARES-8015-E-AC.hpp"
#include "../Applications/Modbus/modbus_slave_core.h"
#include "./optparse/optparse.h"

#define LOG_TAG "motor"     /* 模块TAG */
#define LOG_LVL LOG_LVL_DBG /* 静态过滤级别 */
#include <ulog.h>           /* 必须放在宏定义下面 */

struct rt_event Motor_Class::motor_event;
float Motor_Class::k_velocity2rpm;

static const rt_uint32_t communition_test_data = 0x12345678;

static void Motor_Inited_Thread(void *parameter);

Motor_Class servo[4] = {Motor_Class(1, true), Motor_Class(2, false), Motor_Class(3, false), Motor_Class(4, true)}; /* 分别对应4个象限的电机 */

void Motor_Class::Servo_Init(float max_velocity, float max_rpm)
{
    k_velocity2rpm = max_rpm / max_velocity; /* 计算速度和转速之间的转换系数 */

    rt_err_t rt_result = RT_EOK;
    rt_result = rt_event_init(&Motor_Class::motor_event, "motor event", RT_IPC_FLAG_FIFO); /* 初始化接收事件 */

    struct rt_thread *thread_temp = RT_NULL;
    thread_temp = rt_thread_create("check_motor_init", Motor_Inited_Thread, RT_NULL, 1024, 4, 1);
    if (thread_temp != RT_NULL)
    {
        rt_thread_startup(thread_temp); /* 启动线程 */
    }

    char name[32] = {0};

    for (int i = 0; i < sizeof(servo) / sizeof(Motor_Class); i++)
    {
        servo[i].Init();
        sprintf(name, "servo%d rx thread", i + 1);
        rt_result = rt_thread_init(&servo[i].rx_thread, name, Motor_Class::Servo_RX_Thread, &servo[i], servo[i].rx_thread_stack, sizeof(servo[i].rx_thread_stack), 5, 2); /* 电机接收线程初始化 */
        if (rt_result == RT_EOK)
        {
            rt_thread_startup(&servo[i].rx_thread); /* 启动线程 */
        }
        else
        {
            rt_kprintf("servo%d rx thread error\n", i);
        }

        rt_thread_t thread_temp;
        sprintf(name, "servo%d communication thread", i + 1);
        thread_temp = rt_thread_create(name, Motor_Class::Motor_Communication_Test_Thread, &servo[i], 2048, 8, 2);
        if (thread_temp != RT_NULL)
        {
            rt_thread_startup(thread_temp); /* 启动线程 */
        }
    }
}

rt_err_t Motor_Class::Communication_Test(void) const
{
    return Send(PRIM_EchoTest, communition_test_data >> 16, communition_test_data & 0x0000FFFF); /* 发送测试数据 */
}

void Motor_Class::Set_State(enum Motor_Class::Motor_State_Enum value)
{
    enum Driver_CMD_Code cmd;
    switch (value)
    {
    case Motor_Disable:
        cmd = PRIM_Disable;
        break;
    case Motor_Enable:
        cmd = PRIM_Enable;
        break;
    case Motor_Stop:
        cmd = PRIM_Stop;
        break;
    case Motor_Brake:
        cmd = PRIM_EmergencyStop;
        break;
    default:
        break;
    }
    Send_Broadcast(cmd, 0, 0);
    for (int i = 0; i < sizeof(servo) / sizeof(Motor_Class); i++)
    {
        servo[i].motor_state = value;
    }
}

enum Motor_Class::Motor_State_Enum Motor_Class::Read_State(void) const
{
    return motor_state;
}

void Motor_Class::Clear_Wrong(void) const
{
    Send_Broadcast(PRIM_ClearError, 0, 0);
}

void Motor_Class::Set_Velocity(float velocity) const
{
    int target_rpm;
    target_rpm = (int)(velocity * k_velocity2rpm * 10.0f); /* 设定转速的单位是0.1rpm */
    target_rpm = cw_flag ? target_rpm : -target_rpm;
    Send(PRIM_SetVelocity, target_rpm >> 16, target_rpm & 0x0000FFFF);
}

void Motor_Class::Set_Velocity(int rpm) const
{
    int target_rpm = rpm * 10; /* 设定转速的单位是0.1rpm */
    target_rpm = cw_flag ? target_rpm : -target_rpm;
    Send(PRIM_SetVelocity, target_rpm >> 16, target_rpm & 0x0000FFFF);
}

rt_err_t Motor_Class::Motor_Event_Recv(enum Motor_Class::Motor_Event_Enum event, /* 待接收的事件 */
                                       signed int wait_time,                     /* 接收事件等待时间 */
                                       rt_uint8_t option,                        /* 接收事件选项 */
                                       int motor)
{
    rt_err_t result;
    rt_uint32_t recved;
    rt_uint32_t set = 0;

    if (0 == motor)
    {
        for (int i = 0; i < sizeof(servo) / sizeof(Motor_Class); i++)
        {
            set |= ((rt_uint32_t)(event) << servo[i].event_shift);
        }
    }
    else if (motor <= sizeof(servo) / sizeof(Motor_Class))
    {
        set = ((rt_uint32_t)(event) << servo[motor - 1].event_shift);
    }
    else
    {
        return -RT_ERROR;
    }
    result = rt_event_recv(&Motor_Class::motor_event, set, option, wait_time, &recved);
    return result;
}

void Motor_Class::Init(void)
{
    /* 初始化变量 */
    serial_number = 0;
    current_velocity = 0.0f;
    motor_state = Motor_Disable;
    wrong_code.wrong_code = 0;
    event_shift = (servo_id - 1) * 4;

    char name[32] = {0};
    sprintf(name, "servo%d rx message", servo_id);
    rt_mq_init(&rx_data_message, name, rx_data_message_pool,
               sizeof(CanRxMsg), sizeof(rx_data_message_pool), RT_IPC_FLAG_FIFO);

    Can_Set_Filter(can, servo_id, &rx_data_message); /* 设置滤波器和对应的接收数据邮箱 */

    sprintf(name, "servo%d read v timer", servo_id);
    rt_timer_init(&read_velocity_timer, name, Read_Velocity, this, 5, RT_TIMER_FLAG_PERIODIC | RT_TIMER_FLAG_SOFT_TIMER);

    sprintf(name, "servo%d read wrong timer", servo_id);
    rt_timer_init(&read_wrong_timer, name, Read_Wrong, this, 500, RT_TIMER_FLAG_PERIODIC | RT_TIMER_FLAG_SOFT_TIMER);
}

void Motor_Class::Parse(const CanRxMsg *const rx_data)
{
    const union Motor_Class::Communication_Data *const data = (union Motor_Class::Communication_Data *)rx_data->Data;

    if (servo_id != data->motor_addr)
    {
        return;
    }
    rt_uint16_t xor_temp = 0;
    for (int i = 0; i < 4; i++)
    {
        xor_temp ^= data->word_data[i];
    }
    if (0 != xor_temp)
    {
        return; /* 校验失败 */
    }

    switch (data->cmd)
    {
    case PRIM_GetSerialNo:
        serial_number = (data->para1_high << 24) | (data->para1_low << 16) | (data->para2_high << 8) | (data->para2_low);
        rt_event_send(&motor_event, Init_Completed << event_shift); /* 初始化完成事件 */
        break;
    case PRIM_EchoTest:
    {
        rt_int32_t temp = (data->para1_high << 8) | (data->para1_low);
        temp = (temp << 16) | (data->para2_high << 8) | (data->para2_low);
        if (communition_test_data == temp)
        {
            /* 测试通过 */
            rt_event_send(&motor_event, Communication_OK << event_shift); /* 通信测试通过事件 */
        }
    }
    break;
    case PRIM_GetError:
        wrong_code.wrong_code = (data->para1_high << 8) | (data->para1_low);
        wrong_code.wrong_code = (wrong_code.wrong_code << 16) | (data->para2_high << 8) | (data->para2_low);
        if (wrong_code.wrong_code)
        {
            rt_event_send(&motor_event, Driver_Wrong << event_shift); /* 驱动器报错事件 */
        }
        break;
    case PRIM_GetActVelocity:
    {
        /* 保存数据 */
        rt_int32_t temp = (data->para1_high << 8) | (data->para1_low);
        temp = (temp << 16) | (data->para2_high << 8) | (data->para2_low);
        float v_temp = temp / 10.0f / k_velocity2rpm;
        current_velocity = cw_flag ? v_temp : -v_temp;
    }
    break;
    case PRIM_Enable:
    case PRIM_Disable:
    case PRIM_ClearError:
    case PRIM_EmergencyStop:
    case PRIM_Stop:
    case PRIM_SetVelocity:
    case PRIM_GetParam:
    case PRIM_SetParam:
    case PRIM_SaveParam:
    default:
        break;
    }
}

void Motor_Class::Set_Motor_State(enum Motor_Class::Motor_State_Enum value)
{
    motor_state = value;
    switch (value)
    {
    case Motor_Class::Motor_Disable: /* 电机释放 */
        Send(Motor_Class::PRIM_Disable, 0x0000, 0x0000);
    case Motor_Class::Motor_Enable: /* 电机使能 */
        Send(Motor_Class::PRIM_Enable, 0x0000, 0x0000);
        break;
    case Motor_Class::Motor_Stop: /* 电机停止 */
        Send(Motor_Class::PRIM_Stop, 0x0000, 0x0000);
        break;
    case Motor_Class::Motor_Brake: /* 电机刹车 */
        Send(Motor_Class::PRIM_EmergencyStop, 0x0000, 0x0000);
        break;
    }
}

void Motor_Class::Servo_RX_Thread(void *parameter)
{
    Motor_Class *servo = (Motor_Class *)parameter;
    RT_ASSERT(servo != RT_NULL);

    rt_err_t rt_result;
    CanRxMsg rx_data;

    while (1)
    {
        rt_result = rt_mq_recv(&servo->rx_data_message, &rx_data, sizeof(CanRxMsg), RT_WAITING_FOREVER); /* 接收数据 */
        if (RT_EOK == rt_result)
        {
            /* 接收到了数据 */
            servo->Parse(&rx_data); /* 解析数据 */
        }
    }
}
void Motor_Class::Read_Velocity(void *parameter)
{
    Motor_Class *servo = (Motor_Class *)parameter;
    RT_ASSERT(servo != RT_NULL);
    servo->Send(PRIM_GetActVelocity, 0, 0);
}

void Motor_Class::Read_Wrong(void *parameter)
{
    Motor_Class *servo = (Motor_Class *)parameter;
    RT_ASSERT(servo != RT_NULL);
    servo->Send(PRIM_GetError, 0, 0);
}

rt_err_t Motor_Class::Send(Driver_CMD_Code cmd, unsigned short data1, unsigned short data2) const
{
    union Communication_Data data;
    data.motor_addr = servo_id;
    data.cmd = cmd;
    data.para1_high = data1 >> 8;
    data.para1_low = data1 & 0x00FF;
    data.para2_high = data2 >> 8;
    data.para2_low = data2 & 0x00FF;

    data.word_data[3] = data.word_data[0] ^ data.word_data[1] ^ data.word_data[2];

    return Can_Send(can, data.data, 8, servo_id); /* 通过CANx发送数据，返回发送邮箱 */
}

rt_err_t Motor_Class::Send_Broadcast(Driver_CMD_Code cmd, unsigned short data1, unsigned short data2)
{
    union Communication_Data data;
    data.motor_addr = 0xFF;
    data.cmd = cmd;
    data.para1_high = data1 >> 8;
    data.para1_low = data1 & 0x00FF;
    data.para2_high = data2 >> 8;
    data.para2_low = data2 & 0x00FF;

    data.word_data[3] = data.word_data[0] ^ data.word_data[1] ^ data.word_data[2];

    return Can_Send(CAN2, data.data, 8, 0xFF); /* 通过CANx发送数据，返回发送邮箱 */
}

void Motor_Inited_Thread(void *parameter)
{
    Motor_Class::Motor_Event_Recv(Motor_Class::Init_Completed,
                                  RT_WAITING_FOREVER, RT_EVENT_FLAG_CLEAR | RT_EVENT_FLAG_AND, 0); /* 等待全部电机初始化完成 */
    Set_Init_Flag(Motor_Initialization_Completed);           /* 电机初始化完成 */
    LOG_I("电机初始化完成.");
}

void Motor_Class::Motor_Communication_Test_Thread(void *parameter)
{
    Motor_Class *servo = (Motor_Class *)parameter;
    RT_ASSERT(servo != RT_NULL);

    rt_err_t rt_result;
    while (1)
    {
        servo->Communication_Test();                                                                                  /* 通信测试 */
        rt_result = Motor_Event_Recv(Communication_OK, 500, RT_EVENT_FLAG_CLEAR | RT_EVENT_FLAG_OR, servo->servo_id); /* 等待通信测试通过 */

        if (RT_EOK == rt_result)
        {
            break;
        }
    }

    while (1)
    {
        servo->Send(PRIM_GetSerialNo, 0, 0);                                                  /* 获取驱动器序列号 */
        rt_result = Motor_Event_Recv(Init_Completed, 200, RT_EVENT_FLAG_OR, servo->servo_id); /* 等待初始化完成 */

        if (RT_EOK == rt_result)
        {
            LOG_I("电机%d的序列号为:%ld", servo->servo_id, servo->serial_number); /* 输出序列号 */
            break;
        }
    }

    servo->Set_Motor_State(Motor_Class::Motor_Enable); /* 电机使能 */
    servo->Set_Velocity(0);                            /* 设置0速 */

    rt_timer_start(&servo->read_velocity_timer); /* 启动读速度定时器 */
    rt_timer_start(&servo->read_wrong_timer);    /* 启动读错误代码定时器 */
}

static void help(void)
{
    rt_kprintf("\n");
    rt_kprintf("motor [-h --help] \n");
    rt_kprintf("motor [-r --read] \n");
    rt_kprintf("motor [-w --write] [<motor index>] (<-n --rpm>) (<rpm>) \n");
    rt_kprintf("optional arguments:\n");
    rt_kprintf("  -h, --help      show this help message and exit\n");
    rt_kprintf("  -r, --read      read motor velocity\n");
    rt_kprintf("  -w, --write     write motor velocity\n");
    rt_kprintf("  -n, --rpm       motor rpm\n");
}

static struct optparse_long long_opts[] = {
    {"help", 'h', OPTPARSE_NONE},
    {"read", 'r', OPTPARSE_NONE},
    {"write", 'w', OPTPARSE_REQUIRED},
    {"rpm", 'n', OPTPARSE_REQUIRED},
    {RT_NULL, 0, OPTPARSE_NONE},
};

static rt_err_t motor(int argc, char **argv)
{
    int opt;
    int cmd_index;
    struct optparse options;

    if (argc == 1)
    {
        help();
        return RT_EOK;
    }

    optparse_init(&options, argv);

    int motor_index = 0;
    bool set_motor_index = false;

    while ((opt = optparse_long(&options, long_opts, &cmd_index)) != -1)
    {
        switch (cmd_index)
        {
        case 0: /* help指令 */
            help();
            break;
        case 1: /* read指令 */
        {
            char str[32] = {0};
            for (int i = 0; i < 4; i++)
            {
                sprintf(str, "motor%d velocity:%.2f\n", i + 1, servo[i].current_velocity);
                rt_kprintf(str);
            }
        }
        break;
        case 2: /* write指令 */
            motor_index = atoi(options.optarg);
            set_motor_index = true;
            break;
        case 3: /* value指令 */
        {
            int rpm = atoi(options.optarg);
            if (set_motor_index)
            {
                if (motor_index == 0)
                {
                    for (int i = 0; i < 4; i++)
                    {
                        servo[i].Set_Velocity(rpm); /* 设置电机速度 */
                    }
                }
                else if (motor_index < 5)
                {
                    servo[motor_index - 1].Set_Velocity(rpm); /* 设置电机速度 */
                }
                else
                {
                    rt_kprintf("motor index [0..4]\n");
                }
            }
            else
            {
                rt_kprintf("please input -w --write\n");
            }
        }
        break;
        default:
            if ('?' == opt)
            {
                help();
                rt_kprintf("%s: %s\n", argv[0], options.errmsg);
                return -RT_ERROR;
            }
            break;
        }
    }
    return RT_EOK;
}
MSH_CMD_EXPORT(motor, motor control);
